#!/usr/bin/env python3
import rospy
from localization_module.msg import Position

import time
import math

from robomaster import robot

ep_robot = robot.Robot()
ep_robot.initialize("rndis")
ep_chassis = ep_robot.chassis
ep_chassis.set_pwm_freq(50, 50, 50, 50, 50, 50)
ep_chassis.set_pwm_value(75, 0, 0, 0, 0, 0)

speed_max = 0.3
speed_min = 0.05
angle_speed_max = 20
angle_speed_min = 3
counter = 0
mineral_counter = 0

position_data = [0, 0, 0]

position_mineral_data = [0, 0, 0]

#TODO
planning = [
    [1500, 3900, 1.5*math.pi, 1],
    [900, 5100, 1.5*math.pi, 1],
    [2100, 5100, 1.5*math.pi, 1],
    [2700, 2100, 0.5*math.pi, 0],[2700, 1500, 0.5*math.pi, 1],
    [3300, 900, 0.5*math.pi, 0], [3300, 300, 0.5*math.pi, 1],
    [2100, 900, 0.5*math.pi, 0], [2100, 300, 0.5*math.pi, 1]]

def position_left_callback(data):
    global position_data
    position_data[0] = data.x
    position_data[1] = data.y
    position_data[2] = data.phi + math.pi / 180
    
def position_right_callback(data):
    global position_data
    position_data[0] = data.x
    position_data[1] = data.y
    position_data[2] = data.phi

def position_mineral_callback(data):
    global position_mineral_data
    position_mineral_data[0] = data.x
    position_mineral_data[1] = data.y
    position_mineral_data[2] = data.phi
    
def arrive(delta_distance, delta_phi):
    if (delta_distance < 50 and abs(delta_phi) < math.pi / 90):
        return 1
    else:
        return 0
        
def limit_speed(speed):
    global speed_max
    global speed_min
    if (speed > speed_max):
        return speed_max
    elif (speed < speed_min):
        return speed_min
    else:
        return speed
        
def limit_angle_speed(angle_speed):
    global angle_speed_max
    global angle_speed_min
    if (angle_speed > angle_speed_max):
        return angle_speed_max
    elif (angle_speed < angle_speed_min):
        return angle_speed_min
    else:
        return angle_speed
        
def docking():#TODO
    global mineral_counter
    target_x = -350;
    target_y = -30;
    speed = 0
    delta_distance = 0
    while not rospy.is_shutdown():
        print("docking")
        delta_x = target_x - position_mineral_data[0]
        delta_y = target_y - position_mineral_data[1]
        delta_distance = (delta_x**2 + delta_y**2)**0.5
        print(delta_x ,delta_y)
        if (delta_distance != 0):
            move_x = delta_x / delta_distance
            move_y = delta_y / delta_distance
        else:
            break
        speed = delta_distance * 0.001
        #speed = limit_speed(speed)
        speed_x = move_x * speed
        speed_y = move_y * speed
        print("x", move_x, "y", move_y)
        ep_chassis.drive_speed( - speed_y, - speed_x, 1, 1)#TODO
        time.sleep(1/30)
        if (delta_distance < 20):
            break
    ep_chassis.set_pwm_value(75, 75, 1000, 0, 0, 0)
    ep_chassis.drive_speed(0, -0.4, 0, 1)
    mineral_counter = mineral_counter + 1
    time.sleep(1)
    
def move(target_x, target_y, target_phi):
    delta_x = target_x - position_data[0]
    delta_y = target_y - position_data[1]
    delta_phi = target_phi - position_data[2]
    move_z = 0
    speed = 0
    phi = 0
    print(delta_x)
    #print("delta_phi", delta_phi)
    if (delta_phi > math.pi):
        move_z = - 1
        delta_phi = 2 * math.pi - delta_phi
    elif (delta_phi < - math.pi):
        move_z = 1
        delta_phi = 2 * math.pi - abs(delta_phi)
    elif (delta_phi > 0 and delta_phi < math.pi):
        move_z = 1
        delta_phi = abs(delta_phi)
    else:
        move_z = - 1
        delta_phi = abs(delta_phi)
    delta_distance = ((target_x - position_data[0])**2 + (target_y - position_data[1])**2 )**0.5
    distance = ((delta_x)**2 + (delta_y)**2)**0.5
    if (delta_y > 0 and distance != 0):
        phi = math.acos(delta_x/distance) - position_data[2]
    elif (distance != 0):
        phi = 2 * math.pi - math.acos(delta_x/distance) - position_data[2]
    move_x = math.cos(phi)
    move_y = math.sin(phi)
    print("x:", move_x, "y", move_y)
    print("delta_distance", delta_distance)
    print("speed1", speed)
    speed = delta_distance * 0.001
    angle_speed = delta_phi * 40
    speed = limit_speed(speed)
    angle_speed = limit_angle_speed(angle_speed)
    print(speed)
    speed_x = move_x * speed
    speed_y = move_y * speed
    speed_z = move_z * angle_speed
    #print(speed_x)
    #speed_y = 0
    #speed_z = 0
    ep_chassis.drive_speed(speed_x, - speed_y, - speed_z, 1)
    return move_x, move_y, move_z, delta_distance, delta_phi
    
def back():
    while not rospy.is_shutdown():
        move_x, move_y, move_z, delta_distance, delta_phi = move(800, 2390, 0)
        if(arrive(delta_distance, delta_phi)):
            break
        time.sleep(1/30)
    target_point(750, 2390, 0, 0)
    ep_chassis.set_pwm_value(50, 50, 1000, 0, 0, 0)
    time.sleep(1.5)
    ep_chassis.set_pwm_value(75, 75, 1, 0, 0, 0)
    time.sleep(2)
    target_point(900, 2400, 0, 0)
    target_point(900, 3300, 0, 0)
    target_point(300, 3300, 0, 0)
    target_point(300, 3900, 0, 0)
    target_point(300, 3300, 0, 0)
    target_point(900, 3300, 0, 0)

def target_point(target_x, target_y, target_z, mode):
    while not rospy.is_shutdown():
        move_x, move_y, move_z, delta_distance, delta_phi = move(target_x, target_y, target_z)
        if(arrive(delta_distance, delta_phi)):
            break
        time.sleep(1/30)
    if (mode == 1):
        print("begin docking")
        docking()
        if(mineral_counter < 6):
            back()
        else:
            collect()
    
def collect():
    while not rospy.is_shutdown():
        move_x, move_y, move_z, delta_distance, delta_phi = move(800, 2390, 0)#TODO
        if(arrive(delta_distance, delta_phi)):
            break
        time.sleep(1/30)
    target_point(750, 2390, 0, 0)
    ep_chassis.set_pwm_value(50, 50, 1000, 0, 0, 0)
    time.sleep(1.5)
    ep_chassis.set_pwm_value(75, 75, 1000, 0, 0, 0)
    target_point(900, 2400, 0, 0)
    target_point(900, 1500, 0, 0)
    target_point(240, 1500, 0, 0)
    target_point(240, 2200, 0, 0)
    ep_chassis.drive_wheels(0, 0, 0, 0, 1)
    ep_chassis.set_pwm_value(75, 75, 1, 0, 0, 0)
    time.sleep(120)
    ep_chassis.set_pwm_value(75, 75, 1000, 0, 0, 0)
    target_point(300, 1500, 0, 0)
    target_point(2100, 1000, 0, 0)
    ep_chassis.set_pwm_value(75, 75, 1, 0, 0, 0)

if __name__ == '__main__':
    rospy.init_node("control")
    rospy.Subscriber("position_left", Position, position_left_callback)
    rospy.Subscriber("position_right", Position, position_right_callback)
    rospy.Subscriber("position_mineral", Position, position_mineral_callback)
    rate = rospy.Rate(30)
    #print("init_finished")
    
    while not rospy.is_shutdown():
        target_x = planning[counter][0]
        target_y = planning[counter][1]
        target_z = planning[counter][2]
        target_mode = planning[counter][3];
        target_point(target_x, target_y, target_z, target_mode)
        counter = counter + 1
        if(counter == 9):
            break
        rate.sleep()
    #while not rospy.is_shutdown():
        #pass
        
    ep_chassis.drive_wheels(0, 0, 0, 0, 1)
    ep_robot.close()
